//
// Created by whitby on 8/13/22.
//

#include <iostream>
#include <Eigen/Dense>
#include <fstream>
#include <utility>
#include <vector>

#include "surfel_map.h"

using namespace std;

string pose_path = "/home/whitby/CLionProjects/ORB2DenseMapping/output/pose.txt";
string loop_path = "/home/whitby/CLionProjects/ORB2DenseMapping/output/loop.txt";
string keyframes_path = "/home/whitby/CLionProjects/ORB2DenseMapping/output/path.txt";
string img_path = "/home/whitby/Documents/data/kitti/02/image_0/";
string depth_path = "/home/whitby/Documents/data/kitti/02/depth_0/";
string save_cloud_path = "/home/whitby/Documents/data/kitti/02/mycloud.pcd";
int iteration = 4661;

int main() {
    ifstream fin_pose(pose_path), fin_loop(loop_path), fin_path(keyframes_path);
    int iter = 0;
    Eigen::Matrix3f K = Eigen::Matrix3f::Identity();
    K(0, 0) = 718.856;
    K(1, 1) = 718.856;
    K(0, 2) = 607.1928;
    K(1, 2) = 185.2157;
    SurfelMap surfelMap(1241, 376, K, 60, 0.5, 10);

    while (iter < iteration) {
        cout << "iteration " << iter << endl;
        // read current frame information
        Eigen::Matrix4f publish_pose = Eigen::Matrix4f::Identity();
        int reference_index;
        bool is_keyframe;
        fin_pose >> publish_pose(0, 0) >> publish_pose(0, 1) >> publish_pose(0, 2) >> publish_pose(0, 3)
                 >> publish_pose(1, 0) >> publish_pose(1, 1) >> publish_pose(1, 2) >> publish_pose(1, 3)
                 >> publish_pose(2, 0) >> publish_pose(2, 1) >> publish_pose(2, 2) >> publish_pose(2, 3);
        fin_pose >> reference_index >> is_keyframe;

        // read keyframes information
        vector<Eigen::Matrix4f> allKeyframes;
        int size;
        fin_path >> size;
        while (size-- != 0) {
            Eigen::Matrix4f T_wc = Eigen::Matrix4f::Identity();
            fin_path >> T_wc(0, 0) >> T_wc(0, 1) >> T_wc(0, 2) >> T_wc(0, 3)
                     >> T_wc(1, 0) >> T_wc(1, 1) >> T_wc(1, 2) >> T_wc(1, 3)
                     >> T_wc(2, 0) >> T_wc(2, 1) >> T_wc(2, 2) >> T_wc(2, 3);
            allKeyframes.emplace_back(T_wc);
        }
        cout << "the number of keyframes is " << allKeyframes.size()  << ", current frame is "
             << (is_keyframe ? "a keyframe, " : "not a keyframe, ") << "reference keyframe is " << reference_index << endl;

        // read loop information
        vector<pair<int, int>> loop_info;
        size = 0;
        int first, second;
        fin_loop >> size;
        while (size-- != 0) {
            fin_loop >> first >> second;
            loop_info.emplace_back(first, second);
        }

        cout << "the number of loops is " << loop_info.size() << endl;


        // orb_slam2 information process
        surfelMap.orb_result_process(publish_pose, reference_index, is_keyframe,
                                     allKeyframes, loop_info);

        // read images
        stringstream ss;
        ss << setfill('0') << setw(6) << iter;
        string left_img_path = img_path + ss.str() + ".png";
        string depth_img_path = depth_path + ss.str() + ".tiff";

        cv::Mat imLeft = cv::imread(left_img_path, cv::IMREAD_UNCHANGED);
        cv::Mat imDepth = cv::imread(depth_img_path, cv::IMREAD_UNCHANGED);

        // dense surfel mapping
        surfelMap.synchronize(imLeft, imDepth, reference_index, publish_pose);

        cout << endl;
        iter++;
    }

    // save point cloud
    surfelMap.save_cloud(save_cloud_path);

    fin_path.close();
    fin_loop.close();
    fin_path.close();

    return 0;
}